from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 1. 声明参数（如果需要在命令行指定 YAML 路径，可以声明一个 LaunchArgument）
    declare_param_file_cmd = DeclareLaunchArgument(
        'param_file',
        default_value='/home/c234/cyt/src/zc4_moveit_config/config/robot_description.yaml',
        description='Path to the YAML file containing robot_description'
    )


    # 2. 获取 param_file（默认为 /tmp/robot_description.yaml）
    param_file = LaunchConfiguration('param_file')

    # 3. 启动 robot_state_publisher，使用 param_file 作为参数文件
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        # 下面这种写法表示直接把 "param_file" 当成一个 YAML 参数文件
        parameters=[param_file]
    )

    # 4. 启动 Rviz（如果还需要用 xacro 方式，可以改成你之前的 Command() 方式）
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        output='screen',
        arguments=[
            '-d', 
            os.path.join(get_package_share_directory('zc4_moveit_config'), 'config', 'moveit.rviz')
        ]
    )

    # 5. 包含 move_group.launch.py 
    move_group_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('zc4_moveit_config'), 'launch', 'move_group.launch.py')
        ),
        launch_arguments={
            "kinematics_solver": "kdl_kinematics_plugin/KDLKinematicsPlugin",
            "kinematics_solver_search_resolution": "0.005",
            "kinematics_solver_timeout": "0.005",
        }.items()  
        # 如果需要给 move_group.launch.py 传递额外参数，可以在这个字典里写: {'use_sim_time': 'true'} 等
    )

    # 6. 组装 LaunchDescription
    ld = LaunchDescription()

    ld.add_action(declare_param_file_cmd)
    ld.add_action(robot_state_publisher_node)
    ld.add_action(move_group_launch)
    ld.add_action(rviz_node)
 

    return ld
